%Kevin Kemper

function sysid
clear all

springN		= 1;
save_plots	= 0;

fprintf('Opening log\n');

str = sprintf('response_k%d.log',springN);
log = importdata(str);

tauc	= -0.18;
GR		= 17.25;

t_end = 0.5;

K = 125;
B = 1;
I = 17.2000e-006*GR^2+0.044;
OFFSET	= -0.007351;

sys.tauc = tauc;
sys.GR = GR;

sys.K = K;
sys.B = B;
sys.I = I;
sys.OFFSET = OFFSET;

str = sprintf('./spring%d/sys%d.mat',springN,springN);
save(str,'sys');

fprintf('\n');
fprintf('#define K		%f\n',K);
fprintf('#define B		%f\n',B);
fprintf('#define I		%f\n',I);
fprintf('#define OFFSET	%f\n',OFFSET);

tc = -B/(2*sqrt(K*I))*sqrt(K/I);

t	= log.data(:,1);

motor_angle	= log.data(:,4);
leg_angle	= log.data(:,5);

% figure
% plot(motor_angle-leg_angle +OFFSET)

[tmp,i0]	= max(motor_angle-leg_angle+OFFSET);

t			= t-t(i0);
iend		= find(t >= t_end,1);
t			= t(i0:iend);

motor_angle = motor_angle(i0:iend);
leg_angle	= leg_angle(i0:iend);
delta		= motor_angle-leg_angle +OFFSET;

decay		= (delta(1))*exp(tc*t);



A = [0, 1;
	 -K/I, -B/I];

x0 = [delta(1) 0];

options = odeset('RelTol',1e-3,	'MaxStep',1e-3);

[tsim, x] = ode45(@dynamics, [0, t_end], x0 ,options);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h = figure;
set(h,'position',[ 100 100 400 400])
hold on
plot([t(1) t(end)], [0 0],'K','linewidth',2);
p1 = plot(t, delta,'linewidth',3);
p2 = plot(tsim, x(:,1),'--g','linewidth',3);
p3 = plot(t, decay,'r--','linewidth',2);

% str = sprintf('Spring %d: Angles',springN);
% title(str)

axis tight
legend([p1; p2; p3],'Measured', 'Simulated','Envelope')

xlabel(	'\textbf{Time} $\left( s \hspace{1 mm}\right)$',...
		'interpreter','latex','fontsize',16);

ylabel(	'\textbf{Position} $\left( r a d \hspace{1 mm}\right)$',...
		'interpreter','latex','fontsize',16);

set(gca,'Position',[ 0.15 0.15 0.8 0.8])
	
str = sprintf('/home/drl/projects/jhurst/current/Kemper_journal/images/sysID%d.pdf', springN);
if (save_plots == 1)
	save2pdf(str);
end



% figure
% hold on
% plot(t,log.data(:,6:7),'linewidth',2);
% title('Velocites')
% 
% figure
% hold on
% plot(t,log.data(:,8)*tauc*GR,'linewidth',2);
% title('Torque')


function xdot = dynamics(t, x)
	xdot = A * x;
end
	
	
end
